#include <rclcpp/rclcpp.hpp>  // ROS2的C++客户端库
#include <geometry_msgs/msg/twist.hpp>  // 用于速度指令消息的Twist类型
#include <geometry_msgs/msg/vector3.hpp>  // 用于表示向量的Vector3消息类型
#include <geometry_msgs/msg/pose_stamped.hpp>  // 用于位姿信息的PoseStamped类型
#include <std_msgs/msg/string.hpp>  // 用于发布和订阅String消息
#include <std_msgs/msg/float32_multi_array.hpp>  // 用于订阅浮点数组消息
#include <mavros_msgs/msg/state.hpp>  // 用于无人机状态消息的State类型
#include <math.h>  // 数学库，用于基本数学运算
#include <string>  // C++字符串类
#include <vector>  // C++标准库中的向量类型
#include <Eigen/Dense>  // Eigen库，用于矩阵和向量的操作
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
using namespace std::chrono_literals;  // 使用chrono库的时间单位（如ms）
using std::placeholders::_1;  // 用于订阅回调函数绑定的占位符

// Follower类，继承自rclcpp::Node，用于实现跟随者无人机的功能
class Follower : public rclcpp::Node {
public:
    // 构造函数，接收无人机类型、ID和数量
    Follower()
    : Node("Follow_node"),  // 初始化节点名称
      Kp_(0.5), Kp_avoid_(2.0), vel_max_(10.0), f_(30) {  // 初始化控制参数


        initialize_parameters();
        
        Kp_avoid_ = 2.0;
        namespaces = namespaces + "/";
        // 初始化位姿订阅器，订阅无人机的当前位置
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            uav_type_ + "_" + std::to_string(uav_id_) + "/vision_pose/pose", 1, 
            std::bind(&Follower::pose_callback, this, _1));
        service_status_sub_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(
            namespaces + uav_type_ + "_" + std::to_string(uav_id_) + "Follower_server_topic" , 1, std::bind(&Follower::service_status_callback, this, _1));

        // 初始化避障速度订阅器
        avoid_vel_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
            namespaces + uav_type_ + "_" + std::to_string(uav_id_) + "/avoid_vel", 1, 
            std::bind(&Follower::avoid_vel_callback, this, _1));

        // 初始化编队模式订阅器
        formation_pattern_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
            namespaces +"formation_pattern", rclcpp::QoS(10).best_effort(), 
            std::bind(&Follower::formation_pattern_callback, this, _1));

        // 初始化领队位姿订阅器
        leader_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            uav_type_ + "_0/vision_pose/pose", rclcpp::QoS(10).best_effort(), 
            std::bind(&Follower::leader_pose_callback, this, _1));

        // 初始化速度指令发布器
        vel_enu_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
            namespaces + uav_type_ + "_" + std::to_string(uav_id_) + "/cmd_vel_enu", 1);
        
        // 初始化信息发布器
        info_pub_ = this->create_publisher<std_msgs::msg::String>(
            namespaces + uav_type_ + "_" + std::to_string(uav_id_) + "/info", 1);

        // 初始化命令发布器
        cmd_pub_ = this->create_publisher<std_msgs::msg::String>(
            namespaces  + uav_type_ + "_" + std::to_string(uav_id_) + "/cmd", 1);

        // leader 的速度话题消息
        std::string leader_velocity_topic = namespaces + "leader/velocity";
        leader_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            leader_velocity_topic , rclcpp::QoS(10).best_effort(), 
            std::bind(&Follower::leader_vel_callback, this, _1));
        // follwer 的速度话题消息
        std::string follower_velocity_topic = uav_type_ + "_" + std::to_string(uav_id_) + "/local_position/velocity_local";
        follower_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            follower_velocity_topic, rclcpp::QoS(10).best_effort(), 
            std::bind(&Follower::follower_vel_callback, this, _1));
        delta_t_ = 0.1;

        // 创建定时器，定时调用control_loop函数
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100), std::bind(&Follower::control_loop, this));
    }
    // 订阅器回调函数，用于接收编队模式数据
    void formation_pattern_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg);

    // 订阅器回调函数，更新无人机的当前位姿
    void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

    // 订阅器回调函数，接收领队的位姿信息
    void leader_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 

    // 订阅器回调函数，接收避障速度数据
    void avoid_vel_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) ;

    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    
    void leader_vel_callback(const  geometry_msgs::msg::TwistStamped::SharedPtr msg);
    void follower_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
    
    // 主控制循环，用于计算并发布速度指令
    void control_loop() ;

    void initialize_parameters() {
        // 声明参数，确保名称和类型一致
        this->declare_parameter<int>("vehicle_id", 0);  
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<int>("vehicle_num", 3);

        // 获取参数值
        uav_type_ = this->get_parameter("vehicle_type").as_string();
        namespaces = this->get_parameter("uav_namespace").as_string();
        uav_id_ = this->get_parameter("vehicle_id").as_int();  // 直接获取为整数类型
        uav_num = this->get_parameter("vehicle_num").as_int();  // 获取整数类型的 vehicle_num

        // 输出日志
        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", uav_type_.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle ID: %d", uav_id_);  // 使用 %d 占位符
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", uav_num);  // 使用 %d 占位符
    }

private:
   
    // ROS2的定时器
    rclcpp::TimerBase::SharedPtr timer_;
    // 订阅无人机位姿信息
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    // 订阅避障速度
    rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr avoid_vel_sub_;
    // 订阅编队模式
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr formation_pattern_sub_;
    // 订阅领队位姿
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr leader_pose_sub_;
    
    // 订阅领队的速度
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr leader_velocity_sub_;

    // 订阅本身的速度
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr follower_velocity_sub_;

    // 发布速度指令
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_enu_pub_;
    // 发布信息
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr info_pub_;
    // 发布命令
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_pub_;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_sub_;
    // 存储无人机的当前位姿
    geometry_msgs::msg::PoseStamped pose_;
    // 存储领队的位姿
    geometry_msgs::msg::PoseStamped leader_pose_;
    // 存储避障速度
    geometry_msgs::msg::Vector3 avoid_vel_;
    // 存储编队模式（矩阵形式）
    Eigen::MatrixXf formation_pattern_;
    // 无人机的类型、ID和数量
    std::string uav_type_;
    int uav_id_;
    int uav_num;
    // 控制参数
    double Kp_;
    double Kp_avoid_ = 1.0;
    double vel_max_;
    int f_;
    double diffusion_velocity = 0.58;
    double diffusion_positon = 0.41;
    double diffusion_avoid = 1.0 - diffusion_velocity - diffusion_positon;

    double Kp_p = 56.49;
    double Kp_v =  5.55;
    double delta_t_ = 0.1;

    mavros_msgs::msg::State current_state_;
    rclcpp::Time last_request_;

    std::string namespaces;

    bool service_status = false;
    std::mutex server_status_mutex_;

    std::mutex leader_velocity_mutex;
    geometry_msgs::msg::TwistStamped leader_velocity;

    std::mutex follower_velocity_mutex;
    geometry_msgs::msg::TwistStamped follower_velocity;
};